#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@File    :   fixed_wing_sub_pub.py
@Time    :   2024/05/25 22:47:57
@Author  :   Lin Yuheng 
@Version :   1.0
@Desc    :   None
"""

import os
import sys

sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
# refer to: https://docs.python-guide.org/writing/structure/

from mavros_msgs.msg import (
    AttitudeTarget,
    PositionTarget,
    GlobalPositionTarget,
    State,
    WaypointList,
    WaypointReached,
    Altitude,
    VFR_HUD,
)
from mavros_msgs.srv import (
    CommandBool,
    SetMode,
    WaypointSetCurrent,
    WaypointPull,
    WaypointPush,
    WaypointClear,
)
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu, NavSatFix, BatteryState
from std_msgs.msg import Float64
from geometry_msgs.msg import (
    PoseStamped,
    TwistStamped,
    TwistWithCovarianceStamped,
    AccelWithCovarianceStamped,
)
from fixedwing_sitl.msg import FWstates, FWcmd
from tools.mathlib import *
import numpy as np
import rospy


class FixedWingSubPub:
    def __init__(self):

        # 订阅的数据暂存容器
        self.current_state = State()  # 无人机当前的状态

        self.imu = Imu()

        self.global_position_from_px4 = NavSatFix()

        self.global_rel_alt_from_px4 = Float64()
        
        self.umt_positon_from_px4 = Odometry()

        self.velocity_global_fused_from_px4 = TwistStamped()

        self.velocity_ned_fused_from_px4 = TwistStamped()

        self.local_position_from_px4 = PoseStamped()

        self.acc_ned_from_px4 = AccelWithCovarianceStamped()

        self.wind_estimate_from_px4 = TwistWithCovarianceStamped()

        # self.battery_state_from_px4 = BatteryState()

        self.waypoint_list = WaypointList()

        self.waypoint_reached = WaypointReached()

        self.altitude_from_px4 = Altitude()

        self.air_ground_speed_from_px4 = VFR_HUD()

        self.cmd_from_controller = FWcmd()

        # 服务的暂存容器
        self.mode_cmd = SetMode()

        self.arming_service = CommandBool()

        self.waypoint_set_current_service = WaypointSetCurrent()

        self.waypoint_pull_service = WaypointPull()

        self.waypoint_push_service = WaypointPush()

        self.waypoint_clear_service = WaypointClear()

        # 发布数据的暂存容器
        self.local_pos_sp = PositionTarget()

        self.global_pos_sp = GlobalPositionTarget()
        
        self.att_sp = AttitudeTarget()
        
        self.fw_states_from_mavros = FWstates()
        
        ###
        self.att_sp_Euler = np.zeros(3)
        self.thrust_sp = 0.0
        
        self.PIX_Euler_target = np.zeros(3) # 无人机期望欧拉角（从飞控中读取）
        self.att_angle_Euler = np.zeros(3) # 无人机从当前欧拉角（从飞控中读取）
        
    def get_ros_time(self, begin:rospy.Time):
        time_now = rospy.Time.now()
        currTimeSec = time_now.secs - begin.secs
        currTimenSec = time_now.nsecs / 1e9 - time_now.nsecs / 1e9
        
        return currTimeSec + currTimenSec

    def state_cb(self, msg:State):
        self.current_state = msg
        
    def imu_cb(self, msg:Imu):
        self.imu_data = msg
        q = [0, 0, 0, 0]
        q[0] = msg.orientation.w
        q[1] = msg.orientation.x
        q[2] = msg.orientation.y
        q[3] = msg.orientation.z
        
        self.att_angle_Euler = quat_2_euler(q)
        
    def global_position_from_px4_cb(self, msg:NavSatFix):
        self.global_position_from_px4 = msg
        
    def global_rel_alt_from_px4_cb(self, msg:Float64):
        self.global_rel_alt_from_px4 = msg
        
    def umt_position_from_px4_cb(self, msg:Odometry):
        self.umt_positon_from_px4 = msg
        
    def velocity_global_fused_from_px4_cb(self, msg:TwistStamped):
        self.velocity_global_fused_from_px4 = msg
    
    def velocity_ned_fused_from_px4_cb(self, msg:TwistStamped):
        self.velocity_ned_fused_from_px4 = msg
        
    def local_position_from_px4_cb(self, msg:PoseStamped):
        self.local_position_from_px4 = msg
        
    def wind_estimate_from_px4_cb(self, msg:TwistWithCovarianceStamped):
        self.wind_estimate_from_px4 = msg
        
    # def battery_state_from_px4_cb(self, msg:BatteryState):
    #     self.battery_state_from_px4 = msg
    
    def waypoints_reached_from_px4_cb(self, msg:WaypointReached):
        self.waypoint_reached = msg
    
    def waypointlist_from_px4_cb(self, msg:WaypointList):
        self.waypoint_list = msg

    def altitude_from_px4_cb(self, msg:Altitude):
        self.altitude_from_px4 = msg
        
    def air_ground_speed_from_px4_cb(self, msg:VFR_HUD):
        self.air_ground_speed_from_px4 = msg
        
    def cmd_from_controller_cb(self, msg:FWcmd):
        self.cmd_from_controller = msg